#include <cstdlib>
#include <iostream>
#include <iomanip>
#include <vector>
#include <utility>
#include <numeric>
#include <string>
#include <sstream>
#include <fstream>
#include <stdio.h>
#include <math.h>  //contains M_PI
#include <stdlib.h>
#include <fstream>
#include <algorithm> //min/max_element
#include <stdexcept>
#include <float.h> // for DBL_MAX

#include <boost/ref.hpp>
#include <boost/bind.hpp>
#include <boost/format.hpp>
#include <boost/foreach.hpp>
#include <boost/function.hpp>
#include <boost/filesystem.hpp>


#include <Wt/WSvgImage>
//#include <Wt/WRasterImage>

#include <cv.h>
#include <highgui.h>

#include "genetic_lego/dssim.h"
#include "genetic_lego/LegoRenderer.h"
#include "genetic_lego/LegoDescription.h"
#include "genetic_lego/GeneticEvaluation.h"



extern "C"
{
#include "gaul.h"
}

using namespace std;
using namespace boost;
using namespace boost::posix_time;


//To make the code prettier
#define foreach         BOOST_FOREACH
#define reverse_foreach BOOST_REVERSE_FOREACH





namespace GeneticEvaluation
{

GenOptData::GenOptData( const std::string original_pic )
{
  original_pic_file = original_pic;
  IplImage *img_from_file = cvLoadImage( original_pic.c_str(), CV_LOAD_IMAGE_COLOR );  //does CV_LOAD_IMAGE_COLOR aleways force to be 3 channel uchar image?

  if( !img_from_file )
    throw runtime_error( string("Could not read image file ") + original_pic );

  if( img_from_file->depth!=8 || img_from_file->nChannels!=3 )
  {
    const CvSize imgSize = cvSize( img_from_file->width, img_from_file->height );
    original_image = cvCreateImage( imgSize, 8, 3 );

//    cvCvtColor( img_from_file, original_image, CV_ );
    cvConvertImage( img_from_file, original_image /*, CV_<X>2RGB */);

    assert(0);  //above cvConvertImage is untested, so killing instead for right now - it might be that CV_LOAD_IMAGE_COLOR above will make it so it will never get here
    cvReleaseImage( &img_from_file );
  }else
  {
    original_image = img_from_file;
  }


  if( !original_image )
    throw runtime_error( string("Could not read image file ") + original_pic );

  npixel_x = original_image->width;
  npixel_y = original_image->height;

  lego_colors_only = true;
}//GenOptData constructor


GenOptData::~GenOptData()
{
  if( original_image )
    cvReleaseImage( &original_image );
}//~GenOptData


void perform_genetic_optimization( GenOptData &results )
{
  random_seed(2003);

  const int population_size = results.npopulation;
  const int allele_length = 4;

  GAgeneration_hook generation_hook = GeneticEvaluation::generation_start_hook;
  GAiteration_hook iteration_hook = NULL;  //for non-evolutionary functions
  GAdata_destructor data_destructor = NULL;
  GAdata_ref_incrementor data_ref_incrementor = NULL;
  GAevaluate evaluate = GeneticEvaluation::eval_ga_struggle_score;

  GAseed seed = GeneticEvaluation::seed_initial_parameters;
  GAadapt adapt = NULL;  // GAadapt optimises/performs learning for an entity.
  GAselect_one select_one = ga_select_one_sus; //Selects individual to mutate using Stochastic Universal Sampling selection; fitness values defined as 0.0 is bad, and large positive values are good
  GAselect_two select_two = ga_select_two_sus; //Selects two individuals to mate using Stochastic Universal Sampling selection; fitness values defined as 0.0 is bad, and large positive values are good
  GAmutate mutate = GeneticEvaluation::mutate_ga_params; //ga_mutate_double_singlepoint_drift; //ga_mutate_double_singlepoint_randomize, ga_mutate_double_multipoint, ga_mutate_double_allpoint
//  GAcrossover crossover_type = GeneticEvaluation::do_ga_crossover_mixing;  //gives the 'son' and 'daughter' a parameter from either the 'mother' or 'father'
  GAcrossover crossover_type = ga_crossover_integer_mixing;
  GAreplace replace = NULL;  //inserts a new entity into the population
  vpointer userdata = (void *)&results;

  population *pop = ga_genesis_double( population_size, results.nlegos, allele_length,
                                       generation_hook, iteration_hook,
                                       data_destructor, data_ref_incrementor,
                                       evaluate, seed, adapt, select_one,
                                       select_two, mutate, crossover_type,
                                       replace, userdata );

  const ga_scheme_type scheme = GA_SCHEME_DARWIN;

//http://gaul.sourceforge.net/tutorial/elitism.html:
//  GA_ELITISM_PARENTS_SURVIVE      All parents that rank sufficiently highly will pass to the next generation.
//  GA_ELITISM_ONE_PARENT_SURVIVES	The single fittest parent will pass to the next generation if it ranks sufficiently well.
//  GA_ELITISM_PARENTS_DIE          No parents pass to next generation, regardless of their fitness.
//  GA_ELITISM_RESCORE_PARENTS      All parents are re-evalutated, and those that subsequently rank sufficiently highly will pass to the next generation.
  const ga_elitism_type elitism = GA_ELITISM_ONE_PARENT_SURVIVES;

  const double crossover = 0.9;
  const double mutation  = 0.2;
  const double migration = 0.0;
  ga_population_set_parameters( pop, scheme, elitism, crossover, mutation, migration );

//  ga_evolution( pop, results.maxgenerations );

  setenv( GA_NUM_THREADS_ENVVAR_STRING, "8", 1 );
  ga_evolution_threaded( pop, results.maxgenerations );

//  entity *best = ga_get_entity_from_rank( pop, 0 );
//  double bestchi2 = best->fitness;

  ga_extinction( pop );
}//double perform_genetic_optimization(...)


template<class T> class Image
{
  //adapted from http://www.cs.iit.edu/~agam/cs512/lect-notes/opencv-intro/opencv-intro.html
  private:
  const IplImage* imgp;
  public:
  Image(const IplImage* img=0) {imgp=img;}
  ~Image(){imgp=0;}
  void operator=(IplImage* img) {imgp=img;}
  inline const T* operator[](const int rowIndx) const {
    return ((const T *)(imgp->imageData + rowIndx*imgp->widthStep));}
};

typedef struct{
  unsigned char b,g,r;
} RgbPixel;

typedef struct{
  float b,g,r;
} RgbPixelFloat;

typedef Image<RgbPixel>       RgbImage;
//typedef Image<RgbPixelFloat>  RgbImageFloat;
//typedef Image<unsigned char>  BwImage;
//typedef Image<float>          BwImageFloat;

double pixel_by_pixel_diff( const IplImage *orig_img, const IplImage *test_img )
{
  const int width = orig_img->width;
  const int height = orig_img->height;

  assert( width == test_img->width );
  assert( height == test_img->height );

//  cerr << "orig_img->depth=" << orig_img->depth
//       << ", orig_img->nChannels=" << orig_img->nChannels
//       << ", orig_img->colorModel=" << orig_img->colorModel
//       << ", orig_img->alphaChannel=" << orig_img->alphaChannel << endl;

  const RgbImage a( orig_img ), b( test_img );

  double diff = 0.0;

  for( int x = 0; x < width; ++x )
  {
    for( int y = 0; y < height; ++y )
    {
//      const CvScalar orig = cvGet2D( orig_img, x, y );
//      const CvScalar test = cvGet2D( test_img, x, y );


      const uchar &rdiff = (a[x][y].r > b[x][y].r) ? (a[x][y].r - b[x][y].r)
                                                   : (b[x][y].r - a[x][y].r);
      const uchar &gdiff = (a[x][y].g > b[x][y].g) ? (a[x][y].g - b[x][y].g)
                                                   : (b[x][y].g - a[x][y].g);
      const uchar &bdiff = (a[x][y].b > b[x][y].b) ? (a[x][y].b - b[x][y].b)
                                                   : (b[x][y].b - a[x][y].b);

      diff += (rdiff+gdiff+bdiff)/(16777216.0);  //16777216=256^3
    }//for( int y = 0; y < height; ++y )
  }//for( int x = 0; x < width; ++x )

  if( diff == 0.0 )
    return std::numeric_limits<double>::max();

  return 1.0 / diff;
}//double pixel_by_pixel_diff( orig_img, test_img )


void do_ga_crossover_mixing( population *pop,
                             entity *father, entity *mother,
                             entity *son, entity *daughter )
{
//blah blah blah
}


boost::shared_ptr<LegoSceneDescription> lego_scene( population *pop, entity *dude )
{
  GenOptData *data = (GenOptData *)pop->data;

  boost::shared_ptr<LegoSceneDescription> answer( new LegoSceneDescription() );

  const int nlegos = pop->num_chromosomes;
  answer->pixels_per_cell = data->pixels_per_cell;
  answer->scene_width = data->npixel_x / data->pixels_per_cell;
  answer->scene_height = data->npixel_y / data->pixels_per_cell;

  for( int legon = 0; legon < nlegos; ++legon )
  {
    int *chromosome = ((int *)(dude->chromosome[legon]));

    LegoDescription lego;
    lego.pos_x = chromosome[0];
    lego.pos_y = chromosome[1];

    if( data->lego_colors_only )
    {
      lego.set_color( LegoDescription::LegoColor( chromosome[2] ) );
    }else
    {
      //bellow should be fine for 32 or 64 bit systems
      lego.color[0] = 0xFF & chromosome[2];
      lego.color[1] = 0xFF & (chromosome[2]>>8);
      lego.color[2] = 0xFF & (chromosome[2]>>16);
    }//if( data->lego_colors_only )

    lego.type  = LegoDescription::LegoType( chromosome[3] );

    answer->legos.push_back( lego );
  }//for( int legon = 0; legon < nlegos; ++legon )

  return answer;
}//boost::shared_ptr<LegoSceneDescription> lego_scene( population *pop, entity *dude )


// Called at the beginning of each generation
bool generation_start_hook( const int generation, population *pop )
{
  /*
   *From GAUL documentation, this function is called at the beginning of each
   *  generation by all evolutionary functions. If this callback function
   *  returns FALSE the evolution will terminate.
   *From what I can tell, this function is called with generation==0, after
   *  evaluating
   */


  GenOptData *data = (GenOptData *)pop->data;

  cerr << "on generation " << generation
       << ", pop size=" << pop->size << endl;

//  const int npop = pop->size;
//  entity **entity_iarray = pop->entity_iarray; // The population sorted by fitness.
//  for( int n = 0; n < npop; ++n )
//  {
//    entity *dude = entity_iarray[n];
//    cerr << "\tEntity " << n << " has fitness " << dude->fitness << endl;
//  }//for( int n = 0; n < npop; ++n )

  entity *best = ga_get_entity_from_rank( pop, 0 );
  if( best )
  {
    cerr << "\tbest->fitness=" << best->fitness << endl;
    data->fitnesses.push_back( best->fitness );
    boost::shared_ptr<LegoSceneDescription> scene = lego_scene( pop, best );
    data->best_indiviuals.push_back( scene );

    Wt::WSvgImage renderedImage( data->npixel_x, data->npixel_y );
//    Wt::WRasterImage renderedImage("png", data->npixel_x, data->npixel_y);
    LegoRenderer::paintLegoScene( &renderedImage, scene );
//    const string fname = (boost::format( "gen_%i.png" ) % generation).str();
    const string fname = (boost::format( "gen_%i.svg" ) % generation).str();
    std::ofstream f(fname.c_str(), std::ios::out | std::ios::binary);
    renderedImage.write(f);
  }else cerr << "There was no best for generation " << generation << endl;


  /*
  //Now lets evaulate if we are done with the evaluation
  //We will kinda use similar convergence criteria to the TMVA genetic algorithm
  //  to determine when convergence occurs
  //When the number of improvments within the last m_genNStepMutate
  // a) smaller than m_genNStepImprove, then divide present sigma by m_genSigmaMult
  // b) equal, do nothing
  // c) larger than m_genNStepImprove, then multiply the present sigma by m_genSigmaMult
  //
  //If convergence hasn't improved more than m_genConvergCriteria in the last
  //  m_genConvergNsteps, then consider minimization complete

  const int nstep = data->m_genConvergNsteps;

  vector<double> &chi2s = data->m_generationsBestChi2;

  if( (nstep<=0) || (generation < nstep) || (chi2s.size()<size_t(nstep)) )
  {
    cerr << "generation < data->m_genConvergCriteria" << endl;
    return true;
  }

  vector<double>::const_iterator pos = chi2s.begin() + chi2s.size() - nstep;
  const double minus_n_val = *pos;
  const double improvment = minus_n_val - bestChi2;

  if( improvment < data->m_genConvergCriteria )
  {
    cerr << "Improvment is only " << improvment << " over the last " << nstep
         << " generations, where convergence criteria is "
         << data->m_genConvergCriteria << endl;
    return false;
  }//if( improvment < data->m_genConvergCriteria )

  int num_improvments = -1;
  double prev_chi2 = DBL_MAX;
  for( ; pos != chi2s.end(); ++pos )
  {
    if( ((*pos)+2.0) < prev_chi2 )  //Consider chi2 going down by 2 and improvment - note that this is very add hock
      ++num_improvments;
    prev_chi2 = *pos;
  }//for( ; pos != chi2s.end(); ++pos )

  cerr << "Saw " << num_improvments << " improvments over the last " << nstep
       << " generations, changing sigma from " << data->m_genSigma << " to ";

  if( num_improvments < data->m_genNStepImprove )
    data->m_genSigma /= data->m_genSigmaMult;
  else if( num_improvments > data->m_genNStepImprove )
    data->m_genSigma *= data->m_genSigmaMult;

  cerr << data->m_genSigma << "." << endl;
*/
  return true;
}//bool generation_start_hook( const int generation, population *pop )


void mutate_ga_params( population *pop, entity *father, entity *son )
{
  if( !father || !son )
    die("Null pointer to entity structure passed");

  GenOptData *data = (GenOptData *)pop->data;

  assert( pop->len_chromosomes == 4 );

  // Select mutation locus.
  const int chromo = (int) random_int(pop->num_chromosomes);  //Index of chromosome to mutate  (will allways be 1 for us)
  const int point = (int) random_int(pop->len_chromosomes);   //Index of allele to mutate

  //Copy unchanged data.
  for( int i = 0; i < pop->num_chromosomes; ++i )
  {
    memcpy(son->chromosome[i], father->chromosome[i], pop->len_chromosomes*sizeof(int));
    if( i!=chromo )
      ga_copy_data(pop, son, father, i);
    else
      ga_copy_data(pop, son, NULL, i);
  }//for( int i = 0; i < pop->num_chromosomes; ++i )

  //Mutate by tweaking a single allele.
  int *chromosome = ((int *)(son->chromosome[chromo]));

  const int scene_width = data->npixel_x / data->pixels_per_cell;
  const int scene_height = data->npixel_y / data->pixels_per_cell;


  if( point==0 || point==1 )
  {
    //x position
    const double sigma = data->position_mutation_sigma;
    chromosome[point] += static_cast<int>( floor( sigma * random_unit_gaussian() + 0.5) );

    LegoDescription::LegoType type = LegoDescription::LegoType( chromosome[3] );

    const int width = LegoDescription::ncolomuns( type );
    const int height = LegoDescription::nrows( type );

    if( chromosome[0] < -width )
      chromosome[0] = width;
    else if( chromosome[0] > scene_width )
      chromosome[0] = scene_width;

    if( chromosome[1] < -height )
      chromosome[1] = height;
    else if( chromosome[1] > scene_height )
      chromosome[1] = scene_height;
  }else if( point==2 )
  {
    if( data->lego_colors_only )
    {
      const double sigma = data->color_and_type_sigma;
      const int orig = chromosome[point];
      do
      {
        chromosome[point] = orig + static_cast<int>( floor(sigma*random_unit_gaussian() + 0.5) );
      }while( chromosome[point] >= LegoDescription::kNumLegoColors
              || chromosome[point] < 0 );
    }else
    { //The bitwise and operations bellow are not necassary
      chromosome[2] = (0xFF & random_int_range(0,256));
      chromosome[2] |= (0xFF00 & (random_int_range(0,256)<<8));
      chromosome[2] |= (0xFF0000 & (random_int_range(0,256)<<16));
    }//if( data->lego_colors_only ) / else

  }else if( point==3 )
  {
    const double sigma = data->color_and_type_sigma;
    const int orig = chromosome[point];
    do
    {
      chromosome[point] = orig + static_cast<int>( floor(sigma*random_unit_gaussian() + 0.5) );
    }while( chromosome[point] >= LegoDescription::kNumLegoTypes
            || chromosome[point] < 0 );
  }//if( point==... )
}//void mutate_ga_params( population *pop, entity *father, entity *son )



bool seed_initial_parameters( population *pop, entity *adam )
{
  if( !pop )
    die("Null pointer to population structure passed.");
  if( !adam )
    die("Null pointer to entity structure passed.");

  const int numchromo = pop->num_chromosomes;
  const int chromolen = pop->len_chromosomes;

  assert( chromolen == 4 );

  GenOptData *data = (GenOptData *)pop->data;
  const int scene_width = data->npixel_x / data->pixels_per_cell;
  const int scene_height = data->npixel_y / data->pixels_per_cell;

  for( int legon = 0; legon < numchromo; ++legon )
  {
    int *chromosome = ((int *)(adam->chromosome[legon]));

    chromosome[3] = random_int_range( 0, LegoDescription::kNumLegoTypes );

    LegoDescription::LegoType type = LegoDescription::LegoType( chromosome[3] );
    const int width = LegoDescription::ncolomuns( type );
    const int height = LegoDescription::nrows( type );


    chromosome[0] = random_int_range( -width, scene_width );
    chromosome[1] = random_int_range( -height, scene_height );

    if( data->lego_colors_only )
    {
      chromosome[2] = random_int_range( 0, LegoDescription::kNumLegoColors );
    }else
    { //The bitwise and operations bellow are not necassary
      chromosome[2] = (0xFF & random_int_range(0,256));
      chromosome[2] |= (0xFF00 & (random_int_range(0,256)<<8));
      chromosome[2] |= (0xFF0000 & (random_int_range(0,256)<<16));
    }//if( data->lego_colors_only )
  }//for( int legon = 0; legon < numchromo; ++legon )

  return true;
}//seed_initial_parameters(...)




bool eval_ga_struggle_score( population *pop, entity *dude )
{
  try
  {
    dude->fitness = 0.0;
    GenOptData *data = (GenOptData *)pop->data;
    IplImage *orig = data->original_image;

    boost::shared_ptr<LegoSceneDescription> scene = lego_scene( pop, dude );

    const CvSize imgSize = cvSize( orig->width, orig->height );
    IplImage *img = cvCreateImage( imgSize, orig->depth, orig->nChannels );

    try
    {
      LegoRenderer::paintLegoScene( img, scene );
      dude->fitness = pixel_by_pixel_diff( data->original_image, img );
//      dude->fitness = 1.0/(0.00000001 + dssim( data->original_image, img ));
    }catch(...)
    {
      dude->fitness = 0.0;
      cerr << "Caught exception in eval_ga_struggle_score" << endl;
    }

    cvReleaseImage( &img );
  }catch( std::exception &e )
  {
    cerr <<  "eval_ga_struggle_score(...): caught " << e.what() << endl;
    return false;
  }

  return true;
}//bool eval_ga_struggle_score(population *pop, entity *dude)





}//namespace GeneticEvaluation
